#include <tracker/tracker.h>

/**
 *	Movement callback.
 * 
 * 	This method keeps track of the robots location through its movement updates.
 * 
 * 	Note that movement updates are relative to the old robot position, so we cannot
 * 	simply add the dx and dy values. Instead, they must be rotated to align with the
 * 	orientation of the robot with respect to the absolute coordinate system.
 */
void Tracker::movement_callback (const driver::Movement::ConstPtr& msg)
{
	// Update the estimated position and direction
//	current_x 	+= cosf(current_angle) * msg -> delta_x + sinf(current_angle) * msg -> delta_y;
//	current_y 	+= sinf(current_angle) * msg -> delta_x + cosf(current_angle) * msg -> delta_y;
//	current_angle 	+= msg -> delta_angle;
//	current_angle   = fmod(current_angle, 2 * M_PI); 	// Normalize angle to range [-Pi, +Pi]

	// Calculate distance the center of the robot traveled


	float dist_center = (msg->dist_left + msg->dist_right) / 2.0f;
	float delta_theta = -(msg->dist_right - msg->dist_left) / wheel_base;
	float delta_x = dist_center * cosf(current_angle + delta_theta);
	float delta_y = dist_center * sinf(current_angle + delta_theta);

	current_x += delta_x;
	current_y += delta_y;
	current_angle += delta_theta;
	
	//filter_.robotMoved(msg->delta_x, msg->delta_y, msg->delta_angle);
	filter_.robotMoved(msg->dist_left, msg->dist_right, wheel_base);

	// ros::NodeHandle n;
	// ros::ServiceClient client = n.serviceClient<turtlesim::TeleportAbsolute>("/turtle1/teleport_absolute");
	// turtlesim::TeleportAbsolute srv;
	// srv.request.x = 5 + current_x / 2;
	// srv.request.y = 5 + current_y / 2;
	// srv.request.theta = current_angle;
	// client.call(srv);
	
	// Inform about current location.
	//ROS_INFO("Current location: %f,%f,%f", current_x, current_y, current_angle);
}
